#include "rclcpp/rclcpp.hpp"
#include "std_msgs/msg/float32_multi_array.hpp"
#include "std_msgs/msg/float32.hpp"
#include "rfnn_ekf_cal_v/RFNN_EKF_cal_v.h"
#include "rfnn_ekf_cal_v/RFNN_EKF_cal_v_initialize.h"

double dg,dob,sig,sio,v_o,w_o;
float d=0.07;
float PI=3.1415926536;
float max_rpm=350.0,min_rpm=80.0;
class CalPubSub:public rclcpp::Node
{
    public:
    CalPubSub(std::string name):Node(name)
    {
        RCLCPP_INFO(this->get_logger(), "大家好，我是%s.", name.c_str());
        // 创建一个订阅者订阅话题
        command_subscribe_ = this->create_subscription<std_msgs::msg::Float32MultiArray>("poRec", 10, std::bind(&CalPubSub::vel_callback, this, std::placeholders::_1));
        // 创建发布者L
        command_publisherL_=this->create_publisher<std_msgs::msg::Float32>("cal_vel_l", 10);
        // 创建发布者R
        command_publisherR_=this->create_publisher<std_msgs::msg::Float32>("cal_vel_r", 10);
        //RFNN_EKF_cal_v_initialize();
    }
    private:
        // 声明一个订阅者
        rclcpp::Subscription<std_msgs::msg::Float32MultiArray>::SharedPtr command_subscribe_;
        // 声明话题发布者L
        rclcpp::Publisher<std_msgs::msg::Float32>::SharedPtr command_publisherL_;
        // 声明话题发布者R
        rclcpp::Publisher<std_msgs::msg::Float32>::SharedPtr command_publisherR_;
        // 收到话题数据的回调函数
        void vel_callback(const std_msgs::msg::Float32MultiArray::SharedPtr msg)
        {
            // 创建消息
            std_msgs::msg::Float32 vL_Setpoint1;
            std_msgs::msg::Float32 vR_Setpoint2;
            double vL;
            double vR;
            dg=static_cast<double>(msg->data[0]); // 使用类成员变量
            dob=static_cast<double>(msg->data[1]); // 使用类成员变量
            sig=static_cast<double>(msg->data[2]);
            sio=static_cast<double>(msg->data[3]);
            v_o=static_cast<double>(msg->data[4]);
            w_o=static_cast<double>(msg->data[5]);
            RCLCPP_INFO(this->get_logger(),"target distance=%f, obstacle distance=%f, target angle=%f, obstacle angle=%f",dg,dob,sig,sio);
            RCLCPP_INFO(this->get_logger(),"current linear speed=%f, current angular speed=%f",v_o,w_o);
            if (dg>0.15)
            {
                RFNN_EKF_cal_v(dg,dob,sig,sio,v_o,w_o,&vL,&vR);
                vL_Setpoint1.data=((float)vL)/(d/2.0)/(2*PI)*60.0;
                vR_Setpoint2.data=((float)vR)/(d/2.0)/(2*PI)*60.0;
                /*
                if (vL_Setpoint1.data>0)
                {
                    if (vL_Setpoint1.data>max_rpm)
                    {
                        vL_Setpoint1.data=max_rpm;
                    }
                    else if (vL_Setpoint1.data<min_rpm)
                    {
                        vL_Setpoint1.data=min_rpm;
                    }
                }
                else
                {
                    if (vL_Setpoint1.data<-max_rpm)
                    {
                        vL_Setpoint1.data=-max_rpm;
                    }
                    else if (vL_Setpoint1.data>-min_rpm)
                    {
                        vL_Setpoint1.data=-min_rpm;
                    }
                }
                if (vR_Setpoint2.data>0)
                {
                    if (vR_Setpoint2.data>max_rpm)
                    {
                        vR_Setpoint2.data=max_rpm;
                    }
                    else if (vR_Setpoint2.data<min_rpm)
                    {
                        vR_Setpoint2.data=min_rpm;
                    }
                }
                else
                {
                    if (vR_Setpoint2.data<-max_rpm)
                    {
                        vR_Setpoint2.data=-max_rpm;
                    }
                    else if (vR_Setpoint2.data>-min_rpm)
                    {
                        vR_Setpoint2.data=-min_rpm;
                    }
                }
                */
            }
            else
            {
                vL_Setpoint1.data=0.0;
                vR_Setpoint2.data=0.0;
            }
            RCLCPP_INFO(this->get_logger(),"left speed=%f, right speed=%f",vL_Setpoint1.data,vR_Setpoint2.data);
            command_publisherL_->publish(vL_Setpoint1);
            command_publisherR_->publish(vR_Setpoint2);
        }

};

int main(int argc, char **argv)
{
    static bool initialized = false;
    if (!initialized) {
        RFNN_EKF_cal_v_initialize();
        initialized = true;
    }
    rclcpp::init(argc, argv);
    /*创建对应节点的共享指针对象*/
    auto node = std::make_shared<CalPubSub>("cal_pub_sub");
    /* 运行节点，并检测退出信号*/
    rclcpp::spin(node);
    rclcpp::shutdown();
    return 0;
}